| Note: This tutorial assumes that you have completed the previous tutorials: Writing a Stiffness Controller (C++), Running the Controller. |
 Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags. |
Writing a Force Controller (C++)
Description: Shows how to use the force/impedance controller for force control using C++.Keywords: force, compliance, impedance, stiffness, controller, pr2 arms
Tutorial Level: BEGINNER
Contents
In this tutorial, we will use the force/impedance realtime controller from the ee_cart_imped_control package to execute a trajectory that uses force control on the PR2 arm.
You should first navigate to the directory you created when you wrote the stiffness controller:
roscd ee_cart_imped_tutorial
The Code
Within this package, create the file src/force_control.cpp and paste the following code into it
1 #include <ee_cart_imped_action/ee_cart_imped_arm.hh>
2 #include <ee_cart_imped_msgs/EECartImpedGoal.h>
3
4 int main(int argc, char **argv) {
5 ros::init(argc, argv, "force_control_test");
6
7 /**
8 *The EECartImpedArm class is a wrapper for an action client to the
9 *ee_cart_imped_action server. The argument "r_arm_cart_imped_controller"
10 *tells the client that it is a client for the server that controls the
11 *right arm
12 */
13 EECartImpedArm arm("r_arm_cart_imped_controller");
14
15 /**
16 *This variable will hold the trajectory as we create it.
17 */
18 ee_cart_imped_msgs::EECartImpedGoal traj;
19
20 /**
21 *addTrajectoryPoint is a static function in the EECartImpedArm class that
22 *adds a trajectory point to the end of the first argument. It simply
23 *assigns each value in the goal structure for us to prevent having to
24 *write it all out.
25 *
26 *This is a point in the center of the robot's body. This simply moves the
27 *arm to that point with maximum stiffness.
28 */
29 EECartImpedArm::addTrajectoryPoint(traj, 0.5, 0, 0, 0, 0, 0, 1,
30 1000, 1000, 1000, 30, 30, 30,
31 false, false, false, false, false,
32 false, 4, "/torso_lift_link");
33 /**
34 *Now the PR2 should extend its arm using a constant force in the x
35 *direction until the arm is fully extended.
36 */
37 EECartImpedArm::addTrajectoryPoint(traj, 0.75, 0, 0, 0, 0, 0, 1,
38 10, 1000, 1000, 30, 30, 30,
39 true, false, false, false, false,
40 false, 6, "/torso_lift_link");
41 /**
42 *This is the line that actually sends the trajectory to the action server
43 *and starts the arm moving. The server will block until the arm completes
44 *the trajectory or it is aborted.
45 */
46 arm.startTrajectory(traj);
47 }
Examining the Code
This code is very similar to the code you used in when doing stiffness control. The only change is in the trajectory point we add on line 36:
32 false, 4, "/torso_lift_link");
33 /**
34 *Now the PR2 should extend its arm using a constant force in the x
35 *direction until the arm is fully extended.
36 */
37 EECartImpedArm::addTrajectoryPoint(traj, 0.75, 0, 0, 0, 0, 0, 1,
38 10, 1000, 1000, 30, 30, 30,
39 true, false, false, false, false,
Recall that the arguments to EECartImpedArm::addTrajectoryPoint are (goal, x, y, z, ox, oy, oz, ow, fx, fy, fz, tx, ty, tz, isfx, isfy, isfz, istx, isty, istz, time, frame_id). In the previous tutorial, we gave almost the exact same point. However, we have changed fx from 50 to 10 and isfx from false to true. Therefore, rather than having a small stiffness in the x direction as previously, we are now exerting a constant force of 10 N. Before, the gripper was attempting to maintain a position in x, although only weakly. Now, the gripper will exert a constant force in that direction without trying to achieve any point at all. The result will be, if there is nothing in the way, that the arm will slowly extend to its full range.
Running the Code
If you have not already, you must first download the ee_cart_imped stack.
Then add the following line to the end of the package CMakeLists.txt:
rosbuild_add_executable(force_control src/force_control.cpp)
and remake the package using
rosmake ee_cart_imped_tutorial
Now start the controller and action server. If you have made no changes to the code in ee_cart_imped_control, you do not need to re-start the robot or simulator.
And you are ready to run the code
rosrun ee_cart_imped_tutorial force_control
In the simulator, the PR2 arm should move to the center of the PR2 body and then extend away from the PR2 until it is completely extended. If you are working with the real PR2, you can put your hand in front of the gripper and feel the pressure it is exerting. When you remove your hand the arm should continue to extend.
Remember that the last point is held so the robot will continue exerting a force in the x direction until given a new trajectory.
